#ifndef BSPLINECUBSURF_H_
#include "BSplineCubSurf.h"
#endif

using namespace std;
using std::vector;

BSplineCubSurf :: BSplineCubSurf(GLfloat points[TAMPATCHBSPLINE][TAMPATCHBSPLINE][3]){
 
 copy(&points[0][0][0], &points[0][0][0] + TAMPATCHBSPLINE*TAMPATCHBSPLINE*3, &controlPoints[0][0][0]);
 calculateSurface();
 calculateNormals();
}

BSplineCubSurf :: BSplineCubSurf(){}

void BSplineCubSurf :: calculateSurface(){

 size_t posAcumX = 0;
 size_t posAcumY = 0;

 for (float v=0; v<=1;v+=(float)(1/(float)(TAMBSPLINE-1))){
	posAcumX = 0;
 	for (float u=0; u<=1;u+=(float)(1/(float)(TAMBSPLINE-1))){
		float x = 0;
		float y = 0;
		float z = 0;

		for (size_t i=0; i<=3;i++){
			for (size_t j=0; j<=3;j++){
				float baseIJ = baseBSpline(3,i,u)*baseBSpline(3,j,v);

				x += baseIJ*controlPoints[i][j][0];
				y += baseIJ*controlPoints[i][j][1];
				z += baseIJ*controlPoints[i][j][2];
			}		
		}
		surface[posAcumX][posAcumY][0] = x;
		surface[posAcumX][posAcumY][1] = y;
		surface[posAcumX][posAcumY][2] = z;

		posAcumX++;
 	}
	posAcumY++;
 }
}

//  x0 x1 x2 
//  y0 y1 y2
//
//
// ( x1*y2-x2*y1 ; -(x0*y2-x2*y0) ; x0*y1-x1*y0)
void BSplineCubSurf :: calculateNormals(){

 float h = 0.01;
 for (size_t y=0 ; y<TAMBSPLINE ; y++){
 	for (size_t x=0 ; x<TAMBSPLINE ; x++){	

		if((x>0)&&(x<TAMBSPLINE-1)&&(y>0)&&(y<TAMBSPLINE-1))
			calculateNormalForMiddlePoint(x,y,h);

		else if((x!=TAMBSPLINE-1)&&(y!=TAMBSPLINE-1))
			calculateNormalForPositBorderPoint(x,y,h);

		else if((x!=0)&&(y!=0))
			calculateNormalForNegBorderPoint(x,y,h);	
 	}
 }
}

// fi' = (fi+1 - fi-1)/(2h) centrada
void BSplineCubSurf :: calculateNormalForMiddlePoint(size_t x,size_t y,float h){
	float ant[3] = {surface[x-1][y][0],surface[x-1][y][1],surface[x-1][y][2]};
	float sig[3] = {surface[x+1][y][0],surface[x+1][y][1],surface[x+1][y][2]};

	float dx[3] = {(sig[0]-ant[0])/(2*h),(sig[1]-ant[1])/(2*h),(sig[2]-ant[2])/(2*h)};

	float arr[3] = {surface[x][y+1][0],surface[x][y+1][1],surface[x][y+1][2]};
	float abaj[3] = {surface[x][y-1][0],surface[x][y-1][1],surface[x][y-1][2]};

	float dy[3] = {(arr[0]-abaj[0])/(2*h),(arr[1]-abaj[1])/(2*h),(arr[2]-abaj[2])/(2*h)};

	float normal[3] = {(dx[1]*dy[2]-dx[2]*dy[1]),(dx[2]*dy[0]-dx[0]*dy[2]),(dx[0]*dy[1]-dx[1]*dy[0]) };
	float norm      = sqrt(normal[0]*normal[0] + normal[1]*normal[1] + normal[2]*normal[2]);		
			
	normals[x][y][0] = normal[0]/norm;
	normals[x][y][1] = normal[1]/norm;
	normals[x][y][2] = normal[2]/norm;		
}

// fi' = (fi+1 - fi)/(h) en adelanto
void  BSplineCubSurf :: calculateNormalForPositBorderPoint(size_t x,size_t y,float h){

	float act[3] = {surface[x][y][0],surface[x][y][1],surface[x][y][2]};
	float sig[3] = {surface[x+1][y][0],surface[x+1][y][1],surface[x+1][y][2]};
	float arr[3] = {surface[x][y+1][0],surface[x][y+1][1],surface[x][y+1][2]};

	float dx[3] = {(sig[0]-act[0])/(h),(sig[1]-act[1])/(h),(sig[2]-act[2])/(h)};
	float dy[3] = {(arr[0]-act[0])/(h),(arr[1]-act[1])/(h),(arr[2]-act[2])/(h)};

	float normal[3] = {(dx[1]*dy[2]-dx[2]*dy[1]),(dx[2]*dy[0]-dx[0]*dy[2]),(dx[0]*dy[1]-dx[1]*dy[0]) };
	float norm      = sqrt(normal[0]*normal[0] + normal[1]*normal[1] + normal[2]*normal[2]);		
			
	normals[x][y][0] = normal[0]/norm;
	normals[x][y][1] = normal[1]/norm;
	normals[x][y][2] = normal[2]/norm;		
}

// fi' = (fi - fi-1)/(h) en atraso
void  BSplineCubSurf :: calculateNormalForNegBorderPoint(size_t x,size_t y,float h){

	float act[3] = {surface[x][y][0],surface[x][y][1],surface[x][y][2]};
	float ant[3] = {surface[x-1][y][0],surface[x-1][y][1],surface[x-1][y][2]};
	float abaj[3] = {surface[x][y-1][0],surface[x][y-1][1],surface[x][y-1][2]};

	float dx[3] = {(act[0]-ant[0])/(h),(act[1]-ant[1])/(h),(act[2]-ant[2])/(h)};
	float dy[3] = {(act[0]-abaj[0])/(h),(act[1]-abaj[1])/(h),(act[2]-abaj[2])/(h)};

	float normal[3] = {(dx[1]*dy[2]-dx[2]*dy[1]),(dx[2]*dy[0]-dx[0]*dy[2]),(dx[0]*dy[1]-dx[1]*dy[0]) };
	float norm      = sqrt(normal[0]*normal[0] + normal[1]*normal[1] + normal[2]*normal[2]);		
			
	normals[x][y][0] = normal[0]/norm;
	normals[x][y][1] = normal[1]/norm;
	normals[x][y][2] = normal[2]/norm;		
}

void BSplineCubSurf :: drawNormals(glm::mat4 mat){
 
 for (size_t y=0 ; y<TAMBSPLINE-1 ; y+=DETAILBSPLINESURF){
 	for (size_t x=0 ; x<TAMBSPLINE-1 ; x+=DETAILBSPLINESURF){	
		glBegin(GL_LINES);

		glm::vec4 Position = glm::vec4(surface[x][y][0],surface[x][y][1],surface[x][y][2],1.0) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		glm::vec4 Position2 = glm::vec4(surface[x][y][0]+normals[x][y][0],surface[x][y][1]+normals[x][y][1],surface[x][y][2]+normals[x][y][2],1.0) * mat;
 		glVertex3f(Position2[0],Position2[1],Position2[2]);
	
		 glEnd();
	}
  }
}

void BSplineCubSurf :: drawSurface(glm::mat4 mat){
 for (size_t y=0 ; y<TAMBSPLINE-1 ; y++){
 	glPointSize(1.0);
 	glBegin(GL_TRIANGLE_STRIP);

 	for (size_t x=0 ; x<TAMBSPLINE ; x++){	

		glm::vec4 Position = glm::vec4(surface[x][y][0],surface[x][y][1],surface[x][y][2],1.0) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		glm::vec4 Position2 = glm::vec4(surface[x][y+1][0],surface[x][y+1][1],surface[x][y+1][2],1.0) * mat;
 		glVertex3f(Position2[0],Position2[1],Position2[2]);
 	}
	 glEnd();
 }
}

void BSplineCubSurf :: drawControlPoints(){
 glPointSize(1.0);
 glBegin(GL_POINTS);
 for (size_t i=0;i<TAMPATCHBSPLINE;i++){
	for (size_t j=0;j<TAMPATCHBSPLINE;j++){
		glVertex3f(controlPoints[i][j][0],controlPoints[i][j][1],controlPoints[i][j][2]);
	}
  }
 glEnd();
}

//TODO GENERALIZAR
float BSplineCubSurf :: baseBSpline(size_t n, size_t j,float t){
 float unSexto = 0.1666666666666;

 if (j==0) return unSexto*(-t*t*t+3*t*t-3*t+1);
 if (j==1) return unSexto*(3*t*t*t-6*t*t+4);
 if (j==2) return unSexto*(-3*t*t*t+3*t*t+3*t+1);
 return unSexto*(t*t*t);
}
